#include <pthread.h>

#include "stage.hh"
using namespace Stg;

// generic planner implementation
#include "astar/astar.h"

static const bool verbose = false;

// navigation control params
static const double cruisespeed = 0.4; 
static const double avoidspeed = 0.05; 
static const double avoidturn = 0.5;
static const double minfrontdistance = 0.7;  
static const double stopdist = 0.5;
static const unsigned int avoidduration = 10;
static const unsigned int PAYLOAD = 1;


static unsigned int 
MetersToCell( meters_t m,  meters_t size_m, unsigned int size_c )
{
  m += size_m / 2.0; // shift to local coords
  m /= size_m / (float)size_c; // scale
  return (unsigned int)floor(m); // quantize 
}

static meters_t 
CellToMeters( unsigned int c,  meters_t size_m, unsigned int size_c )
{
  meters_t cell_size = size_m/(float)size_c; 
  meters_t m = c * cell_size; // scale
  m -= size_m / 2.0; // shift to local coords

  return m + cell_size/2.0; // offset to cell center
}


class Robot
{
public: 
	class Task
	{
	public:
		Model* source;
		Model* sink;
		unsigned int participants;
		
		Task( Model* source, Model* sink ) 
			: source(source), sink(sink), participants(0) 
		{}
	};
	
  static std::vector<Task> tasks;

private:
	class Node;
	
	class Edge
	{
	public:
		Node* to;
		double cost;
		
		Edge( Node* to, double cost=1.0 ) 
			: to(to), cost(cost) {}  
	};
	
	class Node
	{
	public:
		Pose pose;
		double value;  
		std::vector<Edge*> edges;
		
		Node( Pose pose, double value=0 )
			: pose(pose), value(value), edges() {}
		
		~Node() 
		{ 
			FOR_EACH( it, edges )
				{ delete *it; }
		}
		
		void AddEdge( Edge* edge ) 
		{ 
			assert(edge);
			edges.push_back( edge ); 	 
		}   
		
		void Draw() const;
	}; 
	
	class Graph
	{
	public:
		std::vector<Node*> nodes;
		
		Graph(){}
		~Graph() { FOR_EACH( it, nodes ){ delete *it; }}
		
		void AddNode( Node* node ){ nodes.push_back( node ); }
  
		void PopFront()
		{
			const std::vector<Node*>::iterator& it = nodes.begin();
			delete *it;
			nodes.erase( it );
		}
		
		void Draw() const
		{ 
			glPointSize(3);
			FOR_EACH( it, nodes ){ (*it)->Draw(); }
		}
		
		bool GoodDirection( const Pose& pose, meters_t range, radians_t& heading_result )
		{
			// find the node with the lowest value within range of the given
			// pose and return the absolute heading from pose to that node 
			
			if( nodes.empty() )
				return 0; // a null guess
			
			
			Node* best_node = NULL;
			
			// find the closest node
			FOR_EACH( it, nodes )
				{
					Node* node = *it;
					double dist = hypot( node->pose.x - pose.x, node->pose.y - pose.y );
		  
					// if it's in range, and either its the first we have found,
					// or it has a lower value than the current best
					if( dist < range &&
							( best_node == NULL || node->value < best_node->value ))
						{
							best_node = node;
						}
				}
	 
			if( best_node == NULL )
				{
					fprintf( stderr, "FASR warning: no nodes in range" );
					return false;
				}

			//else
			heading_result = atan2( best_node->pose.y - pose.y,
															best_node->pose.x - pose.x );
			
			// add a little bias to one side (the left) - creates two lanes
			// of robot traffic
			heading_result = normalize( heading_result + 0.25 );

			return true;
		}
	};

	class GraphVis : public Visualizer
	{
	public:
		Graph** graphpp;
		
		GraphVis( Graph** graphpp ) 
			: Visualizer( "graph", "vis_graph" ), graphpp(graphpp) {}
		virtual ~GraphVis(){}
		
		virtual void Visualize( Model* mod, Camera* cam )
		{
			if( *graphpp == NULL )
				return;
			
			glPushMatrix();
			
			Gl::pose_inverse_shift( mod->GetGlobalPose() );
			
			//mod->PushColor( 1,0,0,1 );
			
			Color c = mod->GetColor();
			c.a = 0.4;
			
			mod->PushColor( c );
			(*graphpp)->Draw();
			mod->PopColor();
			
			glPopMatrix();
		}
	};
	

private:
  long int wait_started_at;
  
  ModelPosition* pos;
  ModelRanger* laser;
  ModelRanger* sonar;
  ModelFiducial* fiducial;
  
  unsigned int task;

  Model* fuel_zone;
  Model* pool_zone;
  int avoidcount, randcount;
  int work_get, work_put;
  bool charger_ahoy;
  double charger_bearing;
  double charger_range;
  double charger_heading;
	
	enum {
		MODE_WORK=0,
		MODE_DOCK,
		MODE_UNDOCK,
		MODE_QUEUE
	} mode;
	
  radians_t docked_angle;

  static pthread_mutex_t planner_mutex;

  Model* goal;
  Pose cached_goal_pose;

  Graph* graphp;
  GraphVis graphvis;
  unsigned int node_interval;
  unsigned int node_interval_countdown;
  
  static const unsigned int map_width;
  static const unsigned int map_height;
  static uint8_t* map_data;
  static Model* map_model;
	 
  bool fiducial_sub;
  bool laser_sub;
  bool sonar_sub;

  bool force_recharge;

public:  

  Robot( ModelPosition* pos, 
				 Model* fuel,
				 Model* pool ) 
		: 
		wait_started_at(-1),
		pos(pos), 
		laser( (ModelRanger*)pos->GetChild( "ranger:1" )),
		sonar( (ModelRanger*)pos->GetChild( "ranger:0" )),
		fiducial( (ModelFiducial*)pos->GetUnusedModelOfType( "fiducial" )),	
		task(random() % tasks.size() ), // choose a task at random
		fuel_zone(fuel),
		pool_zone(pool),
		avoidcount(0), 
		randcount(0), 
		work_get(0), 
		work_put(0),
		charger_ahoy(false),
		charger_bearing(0),
		charger_range(0),
		charger_heading(0),
		mode(MODE_WORK),
		docked_angle(0),
		goal(tasks[task].source),
		cached_goal_pose(),
		graphp(NULL),
		graphvis( &graphp ),
		node_interval( 20 ),
		node_interval_countdown( node_interval ),
		fiducial_sub(false),		
		laser_sub(false),
		sonar_sub(false),
		force_recharge( false )
  {
		// need at least these models to get any work done
		// (pos must be good, as we used it in the initialization list)
		assert( laser );
		assert( fiducial );
		assert( sonar );
		assert( goal );

		// match the color of my destination
		pos->SetColor( tasks[task].source->GetColor() );
		
		tasks[task].participants++;
		
		EnableLaser(true);

		// we access all other data from the position callback
		pos->AddCallback(  Model::CB_UPDATE, (model_callback_t)UpdateCallback, this );
		pos->Subscribe();

		pos->AddVisualizer( &graphvis, true );
	 
		if( map_data == NULL )
			{
				map_data = new uint8_t[map_width*map_height*2];
		  
				// MUST clear the data, since Model::Rasterize() only enters
				// non-zero pixels
				memset( map_data, 0, sizeof(uint8_t) * map_width * map_height);
		  
				// get the map
				map_model = pos->GetWorld()->GetModel( "cave" );
				assert(map_model);
				Geom g = map_model->GetGeom();
		  
				map_model->Rasterize( map_data, 
															map_width, 
															map_height, 
															g.size.x/(float)map_width, 
															g.size.y/(float)map_height );
		  
				// fix the node costs for astar: 0=>1, 1=>9
		  
				unsigned int sz = map_width * map_height;	 
				for( unsigned int i=0; i<sz; i++ )
					{
						if( map_data[i] == 0 )
							map_data[i] = 1;
						else if( map_data[i] == 1 )
							map_data[i] = 9;
						else
							printf( "FASR: bad value %d in map at index %d\n", (int)map_data[i], (int)i );
				
						assert( (map_data[i] == 1) || (map_data[i] == 9) );		  
					}    	 
			}
	 
		//( goal );
		//puts("");
  }
 
	
	void Enable( Stg::Model* model, bool& sub, bool on )
	{
		if( on && !sub )
			{ 
				sub = true;
				model->Subscribe();
			}
		
		if( !on && sub )
			{
				sub = false;
				model->Unsubscribe();
			}
	}
	
  void EnableSonar( bool on )
  { 
		Enable( sonar, sonar_sub, on );
  }

  void EnableLaser( bool on )
  { 
		Enable( laser, laser_sub, on );
  }

  void EnableFiducial( bool on )
  { 
		Enable( fiducial, fiducial_sub, on );
  }

	static std::map< std::pair<uint64_t,uint64_t>, Graph*> plancache;
	

	uint64_t Pt64( const ast::point_t& pt)
	{
		// quantize the position a bit to reduce planning frequency
		uint64_t x = pt.x / 1;
		uint64_t y = pt.y / 1;
		
		return (x<<32) + y;
	}
	
	void CachePlan( const ast::point_t& start, const ast::point_t& goal, Graph* graph )
	{
		std::pair<uint64_t,uint64_t> key( Pt64(start),Pt64(goal));
		plancache[key] = graph;
	}
	
	Graph* LookupPlan( const ast::point_t& start, const ast::point_t& goal )
	{
		std::pair<uint64_t,uint64_t> key(Pt64(start),Pt64(goal));
		return plancache[key];		
	}


 void Plan( Pose sp )
  {
		static float hits = 0;
		static float misses = 0;

		// change my color to that of my destination
		//pos->SetColor( dest->GetColor() );
		
		Pose pose = pos->GetPose();
		Geom g = map_model->GetGeom();
		
		ast::point_t start( MetersToCell(pose.x, g.size.x, map_width), 
									 MetersToCell(pose.y, g.size.y, map_height) );
		ast::point_t goal( MetersToCell(sp.x, g.size.x, map_width),
									MetersToCell(sp.y, g.size.y, map_height) );
		
		//printf( "searching from (%.2f, %.2f) [%d, %d]\n", pose.x, pose.y, start.x, start.y );
		//printf( "searching to   (%.2f, %.2f) [%d, %d]\n", sp.x, sp.y, goal.x, goal.y );
		
		// astar() is not reentrant, so we protect it thus
		
		//graph.nodes.clear(); // whatever happens, we clear the old plan
		

		//pthread_mutex_lock( &planner_mutex );
		
		// check to see if we have a path planned for these positions already
		//printf( "plancache @ %p size %d\n", &plancache, (int)plancache.size() );
		
		//if( graphp )
		//delete graphp;

		graphp = LookupPlan( start, goal );

		if( ! graphp ) // no plan cached
			{
				misses++;
				
				std::vector<ast::point_t> path;
				bool result = ast::astar( map_data, 
																		map_width, 
																		map_height, 
																		start,
																		goal,
																		path );				
				
				if( ! result )
					printf( "FASR2 warning: plan failed to find path from (%.2f,%.2f) to (%.2f,%.2f)\n",
									pose.x, pose.y, sp.x, sp.y );				
				
				graphp = new Graph();
				
				unsigned int dist = 0;
				
				Node* last_node = NULL;

				for( std::vector<ast::point_t>::reverse_iterator rit = path.rbegin();
						 rit != path.rend();
						 ++rit )			
					{	
						//printf( "%d, %d\n", it->x, it->y );
						
						Node* node = new Node( Pose( CellToMeters(rit->x, g.size.x, map_width ),
																				 CellToMeters(rit->y, g.size.y, map_height),
																				 0, 0 ),
																	 dist++ ); // value stored at node
						
						graphp->AddNode( node );
						
 						if( last_node )
 							last_node->AddEdge( new Edge( node ) );
						
 						last_node = node;
					}	 
				
				CachePlan( start, goal, graphp );
			}
		else
			{
				hits++;
				//puts( "FOUND CACHED PLAN" );
			}
				
		//printf( "hits/misses %.2f\n", hits/misses );
		//pthread_mutex_unlock( &planner_mutex );
	}
	
		
  void Dock()
  {
		const meters_t creep_distance = 0.5;
	 
		if( charger_ahoy )
			{
				// drive toward the charger
				// orient term helps to align with the way the charger is pointing
				const double orient = normalize( M_PI - (charger_bearing - charger_heading) );
				const double a_goal = normalize( charger_bearing - 2.0 * orient );
		  		  
				if( charger_range > creep_distance )
					{
						if( !ObstacleAvoid() )
							{
								pos->SetXSpeed( cruisespeed );	  					 
								pos->SetTurnSpeed( a_goal );
							}
					}
				else	// we are very close!
					{			
						pos->SetTurnSpeed( a_goal );
						pos->SetXSpeed( 0.02 );	// creep towards it				 
				
						if( charger_range < 0.08 ) // close enough
							{
								pos->Stop();
								docked_angle = pos->GetPose().a;
								EnableLaser( false );
							}
				
						if( pos->Stalled() ) // touching
							pos->SetXSpeed( -0.01 ); // back off a bit			 				
					}			 
			}			  
		else
			{
				//printf( "FASR: %s docking but can't see a charger\n", pos->Token() );
				pos->Stop();
				EnableFiducial( false );
				mode = MODE_WORK; // should get us back on track eventually
			}

		// if the battery is charged, go back to work
		if( Full() )
			{
				//printf( "fully charged, now back to work\n" );		  
				mode = MODE_UNDOCK;
				EnableSonar(true); // enable the sonar to see behind us
				EnableLaser(true);
				EnableFiducial(true);
				force_recharge = false;		  
			}	 
  }

  void SetTask( unsigned int t )
  {
		task = t;
		tasks[task].participants++;
  }

  void UnDock()
  {
		const meters_t back_off_distance = 0.2;
		const meters_t back_off_speed = -0.02;
		const radians_t undock_rotate_speed = 0.3;
		const meters_t wait_distance = 0.2;
		const unsigned int BACK_SENSOR_FIRST = 10;
		const unsigned int BACK_SENSOR_LAST = 13;
	 
		// make sure the required sensors are running
		assert( sonar_sub );
		assert( fiducial_sub );

		if( charger_range < back_off_distance )
			{
				pos->SetXSpeed( back_off_speed );

				pos->Say( "" );
		  
				// stay put while anything is close behind 
				const std::vector<ModelRanger::Sensor>& sensors = sonar->GetSensors();

				for( unsigned int s = BACK_SENSOR_FIRST; s <= BACK_SENSOR_LAST; ++s )
					if( sensors[s].ranges.size() < 1 || sensors[s].ranges[0] < wait_distance) 
						{
							pos->Say( "Waiting..." );
							pos->SetXSpeed( 0.0 );
							return;
						}	
			}
		else
			{ // we've backed up enough
		  
				double heading_error = normalize( pos->GetPose().a - (docked_angle + M_PI ) );
		  
				if( fabs( heading_error ) > 0.05 ) 
					{
						// turn
						pos->SetXSpeed( 0 );
						pos->SetTurnSpeed( undock_rotate_speed * sgn(-heading_error) );
					}
				else
					{
						// we're pointing the right way, so we're done
						mode = MODE_WORK;  
				
						// if we're not working on a drop-off
						if( pos->GetFlagCount() == 0 )
							{
								// pick a new task at random
								SetTask( random() % tasks.size() );
								SetGoal( tasks[task].source );
							}
						else
							SetGoal( tasks[task].sink );

				
						EnableFiducial(false);
						EnableSonar(false);
					}
			}
  }
	
  bool ObstacleAvoid()
  {
		bool obstruction = false;
		bool stop = false;
  
		// find the closest distance to the left and right and check if
		// there's anything in front
		double minleft = 1e6;
		double minright = 1e6;
  
		// Get the data
		//const std::vector<ModelLaser::Sample>& scan = laser->GetSamples();

		const std::vector<meters_t>& scan = laser->GetSensors()[0].ranges;

    uint32_t sample_count = scan.size();


		for (uint32_t i = 0; i < sample_count; i++)
			{		
				if( verbose ) printf( "%.3f ", scan[i] );
		
				if( (i > (sample_count/4)) 
						&& (i < (sample_count - (sample_count/4))) 
						&& scan[i] < minfrontdistance)
					{
						if( verbose ) puts( "  obstruction!" );
						obstruction = true;
					}
		
				if( scan[i] < stopdist )
					{
						if( verbose ) puts( "  stopping!" );
						stop = true;
					}
		
				if( i > sample_count/2 )
					minleft = std::min( minleft, scan[i] );
				else      
					minright = std::min( minright, scan[i] );
			}
  
		if( verbose ) 
			{
				puts( "" );
				printf( "minleft %.3f \n", minleft );
				printf( "minright %.3f\n ", minright );
			}
  
		if( obstruction || stop || (avoidcount>0) )
			{
				if( verbose ) printf( "Avoid %d\n", avoidcount );
		
				pos->SetXSpeed( stop ? 0.0 : avoidspeed );      
		
				/* once we start avoiding, select a turn direction and stick
					 with it for a few iterations */
				if( avoidcount < 1 )
					{
						if( verbose ) puts( "Avoid START" );
						avoidcount = random() % avoidduration + avoidduration;
			 
						if( minleft < minright  )
							{
								pos->SetTurnSpeed( -avoidturn );
								if( verbose ) printf( "turning right %.2f\n", -avoidturn );
							}
						else
							{
								pos->SetTurnSpeed( +avoidturn );
								if( verbose ) printf( "turning left %2f\n", +avoidturn );
							}
					}			  
		
				avoidcount--;

				return true; // busy avoding obstacles
			}
  
		return false; // didn't have to avoid anything
  }

  
  void SetGoal( Model* goal )
  {	
		if( goal != this->goal )
			{
				this->goal = goal;
				Plan( goal->GetPose() );
				pos->SetColor( goal->GetColor() );
			}
  }
	 
  void Work()
  {
		if( ! ObstacleAvoid() )
			{
				if( verbose ) 
					puts( "Cruise" );
				
				if( Hungry() )
					SetGoal( fuel_zone );
				
				const Pose pose = pos->GetPose();		
				double a_goal = 0;
				
				// if the graph fails to offer advice or the goal has moved a
				// ways since last time we planned
				if( graphp == NULL || 
						(graphp->GoodDirection( pose, 5.0, a_goal ) == 0) || 
						(goal->GetPose().Distance( cached_goal_pose ) > 0.5) )
					{
						//printf( "%s replanning from (%.2f,%.2f) to %s at (%.2f,%.2f) in Work()\n", 
						//	  pos->Token(),
						//	  pose.x, pose.y,
						//	  goal->Token(),
						//	  goal->GetPose().x,
						//	  goal->GetPose().y );
						Plan( goal->GetPose() );
						cached_goal_pose = goal->GetPose();				
					}
		  
				// if we are low on juice - find the direction to the recharger instead
				if( Hungry() )		 
					{ 
						EnableFiducial(true);
				
						while( graphp->GoodDirection( pose, 5.0, a_goal ) == 0 )
							{
								printf( "%s replanning in Work()\n", pos->Token() );
							}
				
				
						if( charger_ahoy ) // I see a charger while hungry!
							mode = MODE_DOCK;
					}
				
				const double a_error = normalize( a_goal - pose.a );
				pos->SetTurnSpeed(  a_error );			
				pos->SetXSpeed( cruisespeed );
			}  
  }


  bool Hungry()
  {
		return( force_recharge || pos->FindPowerPack()->ProportionRemaining() < 0.2 );
  }	 

  bool Full()
  {
		return( pos->FindPowerPack()->ProportionRemaining() > 0.95 );
  }	 
  
  // static callback wrapper
  static int UpdateCallback( ModelRanger* laser, Robot* robot )
  {  
		return robot->Update();
  }
  
  int Update( void )
  {
		if( strcmp( pos->Token(), "position:0") == 0 )
			{
				static int seconds = 0;
		  
				int timenow = pos->GetWorld()->SimTimeNow() / 1e6;
		  
				if( timenow - seconds > 5 )
					{
						seconds = timenow;
				
						// report the task participation		  
						//						printf( "time: %d sec\n", seconds ); 
						  
// 						unsigned int sz = tasks.size(); 
// 						for( unsigned int i=0; i<sz; ++i )
// 							printf( "\t task[%u] %3u (%s)\n", 
// 											i, tasks[i].participants, tasks[i].source->Token() );			 				
					}
			}
	 
		Pose pose = pos->GetPose();

	 
		// assume we can't see the charger
		charger_ahoy = false;
	 
		// if the fiducial is enabled
		if( fiducial_sub ) 
			{	 
				std::vector<ModelFiducial::Fiducial>& fids = fiducial->GetFiducials();
		  
				if( fids.size() > 0 )
					{
						ModelFiducial::Fiducial* closest = NULL;		
				
						for( unsigned int i = 0; i < fids.size(); i++ )
							{				
								//printf( "fiducial %d is %d at %.2f m %.2f radians\n",
								//	  i, f->id, f->range, f->bearing );		
					 
								ModelFiducial::Fiducial* f = &fids[i];
					 
								// find the closest 
								if( f->id == 2 && ( closest == NULL || f->range < closest->range )) // I see a charging station
									closest = f; 
							}						  
		  
						if( closest )
							{			 // record that I've seen it and where it is
								charger_ahoy = true;
					 
								//printf( "AHOY %s\n", pos->Token() );
					 
								charger_bearing = closest->bearing;
								charger_range = closest->range;
								charger_heading = closest->geom.a;			 
							}
					}
			}


		//printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
		//  pose.x, pose.y, pose.z, pose.a );
  
		//pose.z += 0.0001;
		//pos->SetPose( pose );
	 
		if( goal == tasks[task].source )
			{
				// if we're close to the source we get a flag
				Pose sourcepose = goal->GetPose();
				Geom sourcegeom = goal->GetGeom();
		  
				meters_t dest_dist = hypot( sourcepose.x-pose.x, sourcepose.y-pose.y );
		  
				// when we get close enough, we start waiting
				if( dest_dist < sourcegeom.size.x/2.0 ) // nearby
					if( wait_started_at < 0 && pos->GetFlagCount() == 0 ) 
						{
							wait_started_at = pos->GetWorld()->SimTimeNow() / 1e6; // usec to seconds
							//printf( "%s begain waiting at %ld seconds\n", pos->Token(), wait_started_at );
						}
		  
				if( wait_started_at > 0 )
					{
						//long int waited = (pos->GetWorld()->SimTimeNow() / 1e6) - wait_started_at;
				
						// leave with small probability
						if( drand48() < 0.0005 )
							{
								//printf( "%s abandoning task %s after waiting %ld seconds\n",
								//		pos->Token(), goal->Token(), waited );
					 
								force_recharge = true; // forces hungry to return true
								tasks[task].participants--;
								wait_started_at = -1;
								return 0;
							}
					}

				// when we get onto the square
				if( dest_dist < sourcegeom.size.x/2.0 &&				
						pos->GetFlagCount() < PAYLOAD )
					{
				
						// transfer a chunk from source to robot
						pos->PushFlag( goal->PopFlag() );

						if( pos->GetFlagCount() == 1 && wait_started_at > 0 )
							{
								// stop waiting, since we have received our first flag
								wait_started_at = -1;
							} 			 

						if( pos->GetFlagCount() == PAYLOAD )
							SetGoal( tasks[task].sink ); // we're done working
					}
			}	 
		else if( goal == tasks[task].sink )
			{			 
				// if we're close to the sink we lose a flag
				Pose sinkpose = goal->GetPose();
				Geom sinkgeom = goal->GetGeom();
		  
				if( hypot( sinkpose.x-pose.x, sinkpose.y-pose.y ) < sinkgeom.size.x/2.0 && 
						pos->GetFlagCount() > 0 )
					{
						//puts( "dropping" );
						// transfer a chunk between robot and goal

						Model::Flag* f = pos->PopFlag();
						f->SetColor( Color(0,1,0) ); // delivered flags are green
						goal->PushFlag( f );		  
				
						if( pos->GetFlagCount() == 0 ) 
							SetGoal( tasks[task].source ); // we're done dropping off
					}
			}
		else
			{
				assert( goal == fuel_zone || goal == pool_zone );
			}
		  
		switch( mode )
			{
			case MODE_DOCK:
				//puts( "DOCK" );
				Dock();
				break;
		
			case MODE_WORK:
				//puts( "WORK" );
				Work();
				break;

			case MODE_UNDOCK:
				//puts( "UNDOCK" );
				UnDock();
				break;
		
			default:
				printf( "unrecognized mode %u\n", mode );		
			}
  
  
		return 0; // run again
  }
  
  static int FlagIncr( Model* mod, Robot* robot )
	{
		printf( "model %s collected flag\n", mod->Token() );
		return 0;
  }

  static int FlagDecr( Model* mod, Robot* robot )
  {
		printf( "model %s dropped flag\n", mod->Token() );
		return 0;
  }
};


void Robot::Node::Draw() const
{
  // print value
  //char buf[32];
  //snprintf( buf, 32, "%.0f", value );
  //Gl::draw_string( pose.x, pose.y+0.2, 0.1, buf );

  //glBegin( GL_POINTS );
  //glVertex2f( pose.x, pose.y );
  //glEnd();

	glBegin( GL_LINES );
	FOR_EACH( it, edges )
		{ 
			glVertex2f( pose.x, pose.y );
			glVertex2f( (*it)->to->pose.x, (*it)->to->pose.y );
		}
	glEnd();
}


// STATIC VARS
pthread_mutex_t Robot::planner_mutex = PTHREAD_MUTEX_INITIALIZER;
const unsigned int Robot::map_width( 32 );
const unsigned int Robot::map_height( 32 );
uint8_t* Robot::map_data( NULL );
Model* Robot::map_model( NULL );

std::map< std::pair<uint64_t,uint64_t>, Robot::Graph*> Robot::plancache;

std::vector<Robot::Task> Robot::tasks;

void split( const std::string& text, const std::string& separators, std::vector<std::string>& words)
{
  const int n = text.length();
  int start, stop;
  start = text.find_first_not_of(separators);
  while ((start >= 0) && (start < n))
		{
			stop = text.find_first_of(separators, start);
			if ((stop < 0) || (stop > n)) stop = n;
			words.push_back(text.substr(start, stop - start));
			start = text.find_first_not_of(separators, stop+1);
		}
}


// Stage calls this when the model starts up
extern "C" int Init( ModelPosition* mod, CtrlArgs* args )
{  
  // some init for only the first controller
  if( Robot::tasks.size() == 0 )
		{
			srandom( time(NULL) );
			
			// tokenize the worldfile ctrl argument string into words
			std::vector<std::string> words;
			split( args->worldfile, std::string(" \t"), words );			
			// expecting at least one task color name 
			assert( words.size() > 1 );
			
			const World* w = mod->GetWorld();
			
			// make an array of tasks by reading the controller arguments
			for( unsigned int s=1; s<words.size(); s++ )
				Robot::tasks.push_back( Robot::Task( w->GetModel( words[s] + "_source"),
																						 w->GetModel( words[s] + "_sink") ) );			 
		}
	
  new Robot( mod,
						 mod->GetWorld()->GetModel( "fuel_zone" ),
						 mod->GetWorld()->GetModel( "pool_zone" ) );
	
  return 0; //ok
}



